(3) 加components-gripper-suction pad 吸盤-2 << Previous Next >> (5) Python remote API 逆向運動學函式-2

(4) 逆向運動學函式-2

影片:

主控程式

function moving(x,y)
a=0.400 ➛連心線距離
b=0.400
c=math.pow(math.pow(x,2)+math.pow(y,2),0.5)
s=(a+b+c)/2
area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5) ➛運用海龍公式算面積
h=area/(2*c)
deg1_base=math.atan(x/y)
if x<0 and y<0 then
deg1_base=deg1_base+math.pi
end
deg1_tri=math.asin(h/a)
deg1=deg1_base+deg1_tri
deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
deg3=deg2-deg1
sim.setJointTargetPosition(joint01,deg1)
sim.setJointTargetPosition(joint02,-deg2) ➛各軸旋轉角度
sim.setJointTargetPosition(joint03,deg3)

end

function sysCall_threadmain()
joint01=sim.getObjectHandle('joint1')
joint02=sim.getObjectHandle('joint2') ➛呼叫程式代碼
joint03=sim.getObjectHandle('joint3')
jointz=sim.getObjectHandle('jointz')
sim.setJointTargetPosition(joint01,0)
sim.setJointTargetPosition(joint02,0) ➛設定位置
sim.setJointTargetPosition(joint03,0)
sim.setJointTargetPosition(jointz,0)
sim.setIntegerSignal("pad_switch",1) ➛開啟吸盤
sim.setJointTargetPosition(jointz,-0.04) ➛z軸向下吸取，距離0.04
sim.wait(2) ➛等待2秒
sim.setJointTargetPosition(jointz,0)
while sim.getSimulationState()~=sim.simulation_advancing_abouttostopre do ➛迴圈
moving(0.2,0.7) ➛移動到指定位置(藉由上方副程式算出)
sim.wait(2)
sim.setIntegerSignal("pad_switch",0) ➛放下方塊
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.04)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
moving(-0.3,-0.55)
sim.wait(2)
sim.setIntegerSignal("pad_switch",0)
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.04)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
moving(0.1,0.55)
sim.wait(2)
sim.setIntegerSignal("pad_switch",0)
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.04)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
moving(0,0.8)
sim.wait(2)
sim.setIntegerSignal("pad_switch",0)
sim.wait(2)
sim.setIntegerSignal("pad_switch",1)
sim.setJointTargetPosition(jointz,-0.04)
sim.wait(2)
sim.setJointTargetPosition(jointz,0)
sim.wait(2)
end
end

(3) 加components-gripper-suction pad 吸盤-2 << Previous Next >> (5) Python remote API 逆向運動學函式-2